#include "lidar_localization/models/graph_optimizer/g2o/edge/edge_marginalization.hpp"

namespace g2o
{
    EdgeMarginalization::EdgeMarginalization() : BaseMultiEdge<-1, std::pair<Eigen::MatrixXd, Eigen::VectorXd>>()
    {
        vecs_.clear();
        qs_.clear();
    }
    void EdgeMarginalization::setDimension(int d)
    {
        _dimension = d;
        _information.resize(d, d);
        _error.resize(d, 1);
        _measurement.first.resize(d, d);
        _measurement.second.resize(d, 1);
        _information = Eigen::MatrixXd::Identity(d, d);
    }

    void EdgeMarginalization::setSize(int vertices)
    {
        resize(vertices);
        setDimension(vertices * 3);
    }
    void EdgeMarginalization::computeError()
    {
        Eigen::VectorXd dx = Eigen::VectorXd::Zero(_error.rows());
        for (auto it = vecs_.begin(); it != vecs_.end(); it++)
        {
            int index = (*it).first;
            dx.segment(index * 3, 3) = (dynamic_cast<g2o::VertexVec *>(_vertices[index]))->estimate() - (*it).second;
        }
        for (auto it = qs_.begin(); it != qs_.end(); it++)
        {
            int index = (*it).first;
            dx.segment(index * 3, 3) = 2 * ((*it).second.inverse() * (dynamic_cast<g2o::VertexQ *>(_vertices[index]))->estimate()).vec();
        }
        _error = _measurement.first * dx + _measurement.second;
    }
    void EdgeMarginalization::setMeasurement(const std::pair<Eigen::MatrixXd, Eigen::VectorXd> &m)
    {
        _measurement = m;
    }
    void EdgeMarginalization::linearizeOplus()
    {
        for (unsigned int i = 0; i < _vertices.size(); i++)
        {
            _jacobianOplus[i] = _measurement.first.block(0, 3 * i, _dimension, 3);
        }
    }
    void EdgeMarginalization::push_back(int index, const Eigen::Vector3d &v)
    {
        vecs_[index] = v;
    }
    void EdgeMarginalization::push_back(int index, const Eigen::Quaterniond &q)
    {
        qs_[index] = q;
    }
} // namespace g2o